/* 
 * File:   Robot.cpp
 * Author: Piotrek
 * 
 * Created on 2 maj 2010, 23:57
 */
#include "stdafx.h"
#include "Robot.h"
#include "CollisionDetection.h"


Robot::Robot(char * name)
{
    /// @todo wczytywanie modeli (kolejne pliki i ich transformacje) z pliku XML
    if (!strncmp("kawasaki",name,strlen(name)))
    {
#ifdef TRIANGLES
        modules.push_back(Module("kawasaki/podstawa.stl",0,0,0,'x'));
        modules.push_back(Module("kawasaki/czlon_1.stl",0,0,0,'z'));
        modules.push_back(Module("kawasaki/czlon_2.stl",0,100,400,'x'));
        modules.push_back(Module("kawasaki/czlon_3.stl",0,0,770,'x'));
        modules.push_back(Module("kawasaki/czlon_4.stl",0,-30,400,'z'));
        modules.push_back(Module("kawasaki/czlon_5.stl",0,0,310,'x'));
        modules.push_back(Module("kawasaki/czlon_6_flansza.stl",0,-10,0,'z'));
#endif

#ifdef VERTEX_ARRAY
        modules.push_back(Module("kawasaki/podstawa.ply",0,0,0,'x'));
        modules.push_back(Module("kawasaki/czlon_1.ply",0,0,0,'z'));
        modules.push_back(Module("kawasaki/czlon_2.ply",0,100,400,'x'));
        modules.push_back(Module("kawasaki/czlon_3.ply",0,0,770,'x'));
        modules.push_back(Module("kawasaki/czlon_4.ply",0,-30,400,'z'));
        modules.push_back(Module("kawasaki/czlon_5.ply",0,0,310,'x'));
        modules.push_back(Module("kawasaki/czlon_6_flansza.ply",0,-10,0,'z'));
#endif

        modules[0].R= -90.0f;

    }
    else if (!strncmp("mitsubishi",name,strlen(name)))
    {
#ifdef TRIANGLES
        modules.push_back(Module("mitsubishi/czlon_0_podstawaN.stl",0,0,0,'y'));

        /// @bug mitsubishi[1] moze byc niedokladnie
        modules.push_back(Module("mitsubishi/rv-12s_shoulder.stl",0,288,0,'y'));
        Module shoulder_cover("mitsubishi/rv-12s_shoulder_cover.stl");
        shoulder_cover.transform(-62,74,64,'y',-90,I,0);
        shoulder_cover.transform(-62,74,64,'x',20,I,0);
        modules[modules.size() - 1].addTriangles(&shoulder_cover);

        modules.push_back(Module("mitsubishi/czlon_1.stl",82,120,150,'x'));

        // glowne ramie
        modules.push_back(Module("mitsubishi/rv-12s_elbo.stl",0,560,0,'x'));
        modules[modules.size() - 1].addTriangles("mitsubishi/rv-12s_elbo_cover_a.stl",-74,120,0);
        modules[modules.size() - 1].addTriangles("mitsubishi/rv-12sl_j4_ex_flange.stl", -72.5, 80, 261);
        modules[modules.size() - 1].addTriangles("mitsubishi/rv-12s_no2arm.stl", -72.5, 80, 157 + 261);

        Module left_arm_cover("mitsubishi/rv-12s_no2arm_cover_a.stl");
        left_arm_cover.transform(-17,80,418,'y',90,I,0);
        modules[modules.size() - 1].addTriangles(&left_arm_cover);

        Module right_arm_cover("mitsubishi/rv-12s_no2arm_cover_b.stl");
        right_arm_cover.transform(0,0,0,'z',180,I,0);
        right_arm_cover.transform(-128,80,418,'y',-90,I,0);
        modules[modules.size() - 1].addTriangles(&right_arm_cover);

        float tmp1=-65.0;
        modules.push_back(Module("mitsubishi/rv-12s_wrist.stl", -72.5, 80, 318 + 157 + 261+tmp1, 'x'));
        modules[modules.size() - 1].addTriangles("mitsubishi/rv-12s_j6_crbrg_spacer.stl", 0, 0, 0);
        modules[modules.size() - 1].transform(0,0,-tmp1,0,0,I,0);

        modules.push_back(Module("mitsubishi/rv-12s_j6_flange.stl", 0, 0, 26-tmp1, 'z'));

        modules[0].R= 180.0f;
		modules[0].Z = 2000;
#endif

#ifdef VERTEX_ARRAY
		modules.push_back(Module("mitsubishi/czlon_0_podstawaN.ply",0,0,0,'y'));

        /// @bug mitsubishi[1] moze byc niedokladnie
        modules.push_back(Module("mitsubishi/rv-12s_shoulder.ply",0,288,0,'y'));
        Module shoulder_cover("mitsubishi/rv-12s_shoulder_cover.ply");
        shoulder_cover.transform(-62,74,64,'y',-90,I,0);
        shoulder_cover.transform(-62,74,64,'x',20,I,0);
        modules[modules.size() - 1].addTriangles(&shoulder_cover);

        modules.push_back(Module("mitsubishi/czlon_1.ply",82,120,150,'x'));
/*
        // glowne ramie
        modules.push_back(Module("mitsubishi/rv-12s_elbo.ply",0,560,0,'x'));
        modules[modules.size() - 1].addTriangles("mitsubishi/rv-12s_elbo_cover_a.ply",-74,120,0);
        modules[modules.size() - 1].addTriangles("mitsubishi/rv-12sl_j4_ex_flange.ply", -72.5, 80, 261);
        modules[modules.size() - 1].addTriangles("mitsubishi/rv-12s_no2arm.ply", -72.5, 80, 157 + 261);

        Module left_arm_cover("mitsubishi/rv-12s_no2arm_cover_a.ply");
        left_arm_cover.transform(-17,80,418,'y',90,I,0);
        modules[modules.size() - 1].addTriangles(&left_arm_cover);

        Module right_arm_cover("mitsubishi/rv-12s_no2arm_cover_b.ply");
        right_arm_cover.transform(0,0,0,'z',180,I,0);
        right_arm_cover.transform(-128,80,418,'y',-90,I,0);
        modules[modules.size() - 1].addTriangles(&right_arm_cover);

        float tmp1=-65.0;
        modules.push_back(Module("mitsubishi/rv-12s_wrist.ply", -72.5, 80, 318 + 157 + 261+tmp1, 'x'));
        modules[modules.size() - 1].addTriangles("mitsubishi/rv-12s_j6_crbrg_spacer.ply", 0, 0, 0);
        modules[modules.size() - 1].transform(0,0,-tmp1,0,0,I,0);

        modules.push_back(Module("mitsubishi/rv-12s_j6_flange.ply", 0, 0, 26-tmp1, 'z'));
*/
        modules[0].R= 180.0f;
		modules[0].Z = 2000;
	
#endif

    }

	for (unsigned int i=0; i<modules.size(); i++)
	{
		PositionsBuffer[i]=modules[i].R;
	}

    InitializeCollisionDetectionModel();

}
    
/**
 * @brief Translacja modulu do poczatku ukladu wspolrzednych.
 *
 * Operacja ta jest niezbedna aby poprawnie wykonywac obroty poszczegolnych modulow.
 * @note Tymczasowo zaimplementowane tylko dla Kawasaki.
 * @param robot wskaznik do robota "kawasaki"
 * @param module moduł do przetransformowania
 * @param prev indeks modulu poprzedzajacego
 */
void translateToBase(Robot * robot, Module * module, unsigned int prev)
{

    unsigned int i;

    float x=module->X, y=module->Y, z=module->Z;

    // liczymy przesuniecie wzgledem poprzedniego modulu
    for (i=0;i<prev;i++)
    {
        x+=robot->modules[i].X;
        y+=robot->modules[i].Y;
        z+=robot->modules[i].Z;
    }

#ifdef TRIANGLES
    for (i=0;i<module->triangles.size();i++)
    {
        int j;
        for (j=0;j<3;j++)
        {
            module->triangles[i].vertex0[j].x-=x;
            module->triangles[i].vertex0[j].y-=y;
            module->triangles[i].vertex0[j].z-=z;
        }
    }
#endif

#ifdef VERTEX_ARRAY
	float * vertexPtr = module->vertices0;
	for (i=0; i<module->vertexCount; i++)
	{
		*vertexPtr -= x;
		vertexPtr++;
		*vertexPtr -= y;
		vertexPtr++;
		*vertexPtr -= z;
		vertexPtr++;
	}
#endif

}

void Robot::translateModulesToBase()
{
    unsigned int i;
    for (i=1;i<modules.size();i++)
    {
        translateToBase(this,&modules[i],i);
    }
}

void Robot::transform(int updateModule)
{
    unsigned int i;
    
    for (i=updateModule; i<modules.size(); i++)
    {
		if (i==0)
			modules[i].transform(I);
		else
			modules[i].transform(modules[i-1].T);

        this->UpdateCollisionDetectionModel(i);
    }

}

void Robot::updatePositions()
{
	int count = modules.size();
	int updateModel = count;

	/// @todo zadawac z reki polozenia i sprawdzic czy to dziala
	for (unsigned int i=0; i<modules.size(); i++)
	{
		if (PositionsBuffer[i]!=modules[i].R && updateModel==count)
			updateModel = i;

		modules[i].R = PositionsBuffer[i];
	}

	transform(updateModel);
}
